#!/usr/bin/env python
####################################################################
# Author: ziqi han 2021.12.20 #
# Version 0.01 #
# Open Loop Control
# Key points : TF + cmd_vel
# TODO: Avoid Obstacles using Lidar
####################################################################

import rospy
import time
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi, atan2,sin,cos
from tf.transformations import euler_from_quaternion

msg = """
control your Leader!
-----------------------
this is Robot3
-----------------------
"""

binge=1
robot1_vel=Twist()
# robot1_rot=Point()
robot1_pos=Point()

robot2_pos=Point()
robot2_vel=Twist()
robot3_pos=Point()
# attacker_pos=Point()

K1=0.2
K2=0.1

detect_R=2
safe_r1=0.2
safe_r2=0.5
ID=0

class GotoPoint():
    def __init__(self):
        rospy.init_node('robot3_vel', anonymous=False)

        rospy.on_shutdown(self.shutdown)

        # Publisher
        self.cmd_vel = rospy.Publisher('robot3/cmd_vel', Twist, queue_size=5)
        self.robot2_positon=rospy.Publisher('/robot3_pos',Point,queue_size=5)

        position = Point()
        move_cmd = Twist()
        r = rospy.Rate(10)
        self.tf_listener = tf.TransformListener()
        self.odom_frame = '/robot3/odom'
        
        # TF
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/robot3/base_link', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/robot3/base_link'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")
            rospy.signal_shutdown("tf Exception")
        
        global robot1_pos
        global robot2_pos
        global robot3_pos
        # global attacker_pos
        avo=1

        # Update and broadcast self position 
        (position, rotation) = self.get_odom()

        rospy.Subscriber('/robot1_pos',Point,update_leader_position)   #leader
        rospy.Subscriber('/robot2_pos',Point,update_robot2_position)   
        # rospy.Subscriber('/attacker_pos',Point,point_callback_6)

        # rospy.Subscriber('/robot1/cmd_rot',Point,update_leader_ori)
        rospy.Subscriber('cmd_vel',Twist,update_leader_vel)

        # For Close Loop Control
        self.robot3_positon.publish(position)
        
        # Heuristic function
        robot3_vel_delta_x = robot1_pos.x - position.x - 0.5
        robot3_vel_delta_y = robot1_pos.y - position.y - 0.5 * 1.732

        if abs(robot3_vel_delta_x) < 0.05:
            robot3_vel_delta_x = 0
        if abs(robot3_vel_delta_y) < 0.05:
            robot3_vel_delta_y = 0

        goal_x = robot3_vel_delta_x + position.x
        goal_y = robot3_vel_delta_y + position.y
        goal_z = atan2(robot3_vel_delta_y,robot3_vel_delta_x)

        if robot3_vel_delta_x==0 and robot3_vel_delta_y==0:
            goal_z = 0
        print ('goal_x=%f'%(goal_x))
        print ('goal_y=%f'%(goal_y))
        print ('aim_ang1=%f'%(goal_z))

        distance = sqrt(pow(robot3_vel_delta_x, 2) + pow(robot3_vel_delta_y, 2))

        print ('robot3_vel_data_x=%f'%(robot3_vel_delta_x))
        print ('robot3_vel_data_y=%f'%(robot3_vel_delta_y))
        print ('distance=%f'%(distance))
        
        global robot1_vel
        angular_now = rotation
        phi = goal_z

        robot3_pos.x = position.x - 0.5
        robot3_pos.y = position.y - 0.5 * 1.732
        # attacker_pos.x=attacker_pos.x+1
        # attacker_pos.y=attacker_pos.y-1
        
        # pos_nodes=[robot2_pos,robot3_pos,robot1_pos,attacker_pos]
        pos_nodes=[robot3_pos,robot1_pos]

        temp_x_sum=0
        temp_x=0
        temp_y_sum=0
        temp_y=0
        print(pos_nodes)

        for i in range(len(pos_nodes)):
            if i!= ID:
                # print i
                # print pos_nodes[i].x

                r=sqrt(pow(pos_nodes[i].x-position.x,2)+pow(pos_nodes[i].y-position.y,2))
                if r < detect_R:
                    print ('distance between %d is %f'%(i,r))
                    print (i)
                    temp_x=(1/r-1/detect_R)*(pos_nodes[ID].x-pos_nodes[i].x)
                    temp_y=(1/r-1/detect_R)*(pos_nodes[ID].y-pos_nodes[i].y)
                    temp_fenmu=pow(r,3)
                    temp_x=temp_x/temp_fenmu
                    temp_y=temp_y/temp_fenmu
                    temp_x_sum=temp_x_sum+temp_x
                    temp_y_sum=temp_y_sum+temp_y
                    if r > safe_r2:
                        avo=2
                    elif r < safe_r1:
                        avo=5
                    else:
                        avo=3
        avoid_delta=temp_x_sum*sin(angular_now)+temp_y_sum*cos(angular_now)                 
        print ('the avoid_delta=%f'%(avoid_delta))

        delta_theta=self.compute_theta(phi,angular_now)
        print ('the delta_ang=%f'%(delta_theta))
        print ('the ang_now=%f'%(angular_now))
        print ('the goal_angular=%f'%(phi))

        if abs(delta_theta)>1:
            robot3_vel.linear.x = 0 + avo*avoid_delta
            robot3_vel.angular.z = delta_theta*2
        else:
            # Modify K1 & K2
            robot3_vel.linear.x = K1*(robot3_vel_delta_x*cos(angular_now)+robot3_vel_delta_y*sin(angular_now)) + robot1_vel.linear.x + avo*avoid_delta
            robot3_vel.angular.z = K2*delta_theta

        print ('the vel_x=%f'%(robot3_vel.linear.x))
        print ('the vel_angular=%f'%(robot3_vel.angular.z))

        # publish robot3 velocity
        self.cmd_vel.publish(robot3_vel) 

        # (position, rotation) = self.get_odom()
        msgs ="""this is robot3_theta"""
        print(msgs)
        
        
    def compute_theta(self,theta,rotation1):
        if theta*rotation1<0:
            if theta>0:
                if abs(rotation1)+theta<=pi:
                    w=abs(rotation1)+theta
                else:
                    w=-(2*pi+rotation1-theta)
            else:
                if rotation1+abs(theta)<=pi:
                    w=-(abs(theta)+rotation1)
                else:
                    w=(2*pi-rotation1+theta)
        else:
            w=theta-rotation1
        return w


    def get_odom(self):
        try:
            (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
            rotation = euler_from_quaternion(rot)

        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), rotation[2])
    def shutdown(self):
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)


def update_leader_vel(data):
    global robot1_vel
    robot1_vel=data

def update_leader_position(data):
    global robot1_pos
    robot1_pos.x = data.x
    robot1_pos.y = data.y

# def update_leader_ori(data):
#     global robot1_rot
#     robot1_rot=data

def update_robot2_position(data):
    global robot2_pos
    robot2_pos.x=data.x
    robot2_pos.y=data.y

# def point_callback_6(data):
#     global attacker_pos
#     attacker_pos.x=data.x
#     attacker_pos.y=data.y


if __name__ == '__main__':
	try:
		while not rospy.is_shutdown():
			print(msg)
			binge = binge+1
			GotoPoint()
	except:
		rospy.loginfo("shutdown program.")